

#include "vector.h"
#include "matrix.h"

#include <math.h>


void vm_transform(vm_vector *v, vm_matrix *m, vm_vector *dest){

  vm_matrix *vect_m = vm_make_matrix(4, 1);
  vect_m->data[0][0] = v->x;
  vect_m->data[1][0] = v->y;
  vect_m->data[2][0] = v->z;
  vect_m->data[3][0] = v->w;

  /*printf("[vm_transform] matrix vector: \n");
  vm_print_matrix(vect_m, " ");*/

  vm_matrix *trans = vm_matrix_mult(m, vect_m);
  if (trans == NULL){
    fprintf(stderr, "Error in transformation.\n");
    return;
  }  

  /*printf("[vm_transform] transformed matrix vector: \n");
  vm_print_matrix(trans, " ");*/

  dest->x = trans->data[0][0];
  dest->y = trans->data[1][0];
  dest->z = trans->data[2][0];

  vm_free_matrix(vect_m);
  vm_free_matrix(trans);

}

vm_matrix *vm_make_rotation_mtx(vm_vector n, double t){

  vm_matrix *rot = vm_make_matrix(4, 4);

  rot->data[0][0] = (n.x * n.x * (1 - cos(t))) + cos(t); 
  rot->data[0][1] = (n.x * n.y * (1 - cos(t))) + n.z * sin(t);
  rot->data[0][2] = (n.x * n.z * (1 - cos(t))) - n.y * sin(t);

  rot->data[1][0] = (n.y * n.x * (1 - cos(t))) - n.z * sin(t); 
  rot->data[1][1] = (n.y * n.y * (1 - cos(t))) + cos(t);
  rot->data[1][2] = (n.y * n.z * (1 - cos(t))) - n.x * sin(t);

  rot->data[2][0] = (n.z * n.x * (1 - cos(t))) - n.y * sin(t); 
  rot->data[2][1] = (n.z * n.y * (1 - cos(t))) - n.x * sin(t);
  rot->data[2][2] = (n.z * n.z * (1 - cos(t))) - cos(t);

  rot->data[3][3] = 1;

  return rot;

}

vm_matrix *vm_make_scale_matrix(vm_vector n, double k){

  vm_matrix *scale = vm_make_matrix(4, 4);

  scale->data[0][0] = 1 + ((k - 1) * n.x * n.x);
  scale->data[0][1] = (k - 1) * n.x * n.y;
  scale->data[0][2] = (k - 1) * n.x * n.z;

  scale->data[1][0] = (k - 1) * n.x * n.y;
  scale->data[1][1] = 1 + ((k - 1) * n.y * n.y);
  scale->data[1][2] = (k - 1) * n.y * n.z;

  scale->data[2][0] = (k - 1) * n.x * n.z;
  scale->data[2][1] = (k - 1) * n.y * n.z;
  scale->data[2][2] = 1 + ((k - 1) * n.z * n.z);

  scale->data[3][3] = 1;
  
  return scale;

}

vm_matrix *vm_make_projection_mtx(vm_vector n){

  vm_matrix *proj = vm_make_matrix(4, 4);

  proj->data[0][0] = 1 - (n.x * n.x);
  proj->data[0][1] = -1 * n.x * n.y;
  proj->data[0][2] = -1 * n.x * n.z;

  proj->data[1][0] = -1 * n.x * n.y;
  proj->data[1][1] = 1 - (n.y * n.y);
  proj->data[1][2] = -1 * n.y * n.z;

  proj->data[2][0] = -1 * n.x * n.z;
  proj->data[2][1] = -1 * n.y * n.z;
  proj->data[2][2] = 1 - (n.z * n.z);

  proj->data[3][3] = 1;

  return proj;

}

vm_matrix *vm_make_reflection_mtx(vm_vector n){

  vm_matrix *ref = vm_make_matrix(4, 4);

  ref->data[0][0] = 1 - (2 * n.x * n.x);
  ref->data[0][1] = -2 * n.x * n.y;
  ref->data[0][2] = -2 * n.x * n.z;

  ref->data[1][0] = -2 * n.y * n.x;
  ref->data[1][1] = 1 - (2 * n.y * n.y);
  ref->data[1][2] = -2 * n.y * n.z;

  ref->data[2][0] = -2 * n.z * n.x;
  ref->data[2][1] = -2 * n.z * n.y;
  ref->data[2][2] = 1 - (2 * n.z * n.z);

  ref->data[3][3] = 1;

  return ref;

}

vm_matrix *vm_make_x_rot_mtx(double theta){

  vm_matrix *m = vm_make_matrix(4, 4);

  m->data[0][0] = 1;

  m->data[1][1] = cos(theta);
  m->data[1][2] = sin(theta);
  
  m->data[2][1] = - sin(theta);
  m->data[2][2] = cos(theta);

  m->data[3][3] = 1;

  return m;

}

vm_matrix *vm_make_y_rot_mtx(double theta){

  vm_matrix *m = vm_make_matrix(4, 4);

  m->data[1][1] = 1;

  m->data[0][0] = cos(theta);
  m->data[1][2] = - sin(theta);
  
  m->data[2][0] = sin(theta);
  m->data[2][2] = cos(theta);

  m->data[3][3] = 1;

  return m;

}

vm_matrix *vm_make_z_rot_mtx(double theta){

  vm_matrix *m = vm_make_matrix(4, 4);

  m->data[2][2] = 1;

  m->data[0][0] = cos(theta);
  m->data[0][1] = sin(theta);
  
  m->data[1][0] = - sin(theta);
  m->data[1][1] = cos(theta);

  m->data[3][3] = 1;

  return m;

}

/**
 * This expects three linearly independent vectors defining a coordinate space.
 * It returns a matrix that maps those vectors to I, J, and K.
 */
vm_matrix *vm_make_space_trans_mtx(vm_vector *vects){

  vm_matrix *trans = vm_make_matrix(4, 4);
  
  trans->data[0][0] = vects[0].x;
  trans->data[0][1] = vects[1].x;
  trans->data[0][2] = vects[2].x;

  trans->data[1][0] = vects[0].y;
  trans->data[1][1] = vects[1].y;
  trans->data[1][2] = vects[2].y;

  trans->data[2][0] = vects[0].z;
  trans->data[2][1] = vects[1].z;
  trans->data[2][2] = vects[2].z;

  trans->data[3][0] = 0;
  trans->data[3][1] = 0;
  trans->data[3][2] = 0;
  trans->data[3][3] = 1;

  vm_matrix *inv_trans = vm_inverse(trans);
  vm_free_matrix(trans);

  return inv_trans;

}
